/* 
 Multiwiicopter LED Ring
  - I2C implementation for Multiwii
  - standalone implementation 
  
 by Alexander Dubus
 adapted by Shikra
 folded, spindled, and mutilated by kapu - November 2014
      
 This is kapu's bastardized version 1.0.  You may do what you wish with it.
 This code has only been tested with LED Ring v3 and MultiWii v2.3
      
 For documentation and files related to standard LED Ring code, see:
 http://code.google.com/p/ledring/ 
 
 For LED Ring hardware specs, schematics and additional code examples, check out
 the manufacturer's wiki:
 http://www.dfrobot.com/wiki/index.php/Rainbow_Ring_V3_(SKU:DFR0141) 
 */

/****    Definitions and Declarations   **************************************************************************/

/* LED board inverted - uncomment to reverse MAG direction */
//#define reverse_mag

/* The LED board I2C address */
#define I2C_address 0x6D

#include <Wire.h>
#include <EEPROM.h>
#include <PinChangeInt.h>  // external library repository: http://code.google.com/p/arduino-pinchangeint

#define RedA PORTD5
#define RedB PORTC1
#define GreenA PORTD6
#define GreenB PORTC0
#define BlueA PORTD7
#define BlueB PORTC2
#define ALED ((1<<RedA)|(1<<GreenA)|(1<<BlueA))
#define BLED ((1<<RedB)|(1<<GreenB)|(1<<BlueB))
#define AllOn ((1<<PORTB0)|(1<<PORTB1)|(1<<PORTB2)|(1<<PORTB3)|(1<<PORTB4)|(1<<PORTB5))

#define NO_PORTB_PINCHANGES // Disable pin change interrupts on PortB to save memory
#define NO_PORTC_PINCHANGES // Disable pin change interrupts on PortC to save memory

#define maxSequence 31 // <---set to highest numbered pre-defined LED sequence

uint8_t brightness[3][12];    // 12 LEDS - 3 COLORS - 256 brightness levels per color (max brightness = 255) 
uint8_t copybright[3][12]; 
int16_t	param[7];            // Parameters passed from Multiwii 
int16_t magLED=0;
//int val;
uint32_t ledlastflashtime,timer;
uint8_t Switch1=3;               // Arduino Pin-->Switch1 assignment
uint8_t Switch2=4;               // Arduino Pin-->Switch2 assignment  
uint8_t Switch1_val=0;           // Switch1 value. Selects LED sequence in standalone mode
uint8_t Switch2_val=1;           // Switch2 value. 0=I2C mode. 1=standalone mode. 2=demo mode.
uint8_t red,green,blue,rgb,led,pos,i,var,start;

/****    Main program   **************************************************************************/

void setup() {
  InitIO();  
  pinMode(Switch1,INPUT);     
  pinMode(Switch2,INPUT);
  digitalWrite(Switch1,HIGH);
  digitalWrite(Switch2,HIGH);
  PCintPort::attachInterrupt(Switch1,S1,RISING); // configure PinChange Interrupt for Switch1 
  PCintPort::attachInterrupt(Switch2,S2,RISING); // configure PinChange Interrupt for Switch2 
  Wire.begin(I2C_address);         
  Wire.onReceive(receiveEvent);
  eepromRead(); // retrieve last saved mode and standalone LED sequence
}

void loop() {
  if (Switch2_val==0) 
    I2C_mode();
  else
    if (Switch2_val==1)
      Standalone_mode();
    else 
      Demo_mode();
  wait(50); 
}

/**** Control Modes  *******************************************************************************************/

// In standalone mode, Switch1 toggles through defined LED sequences
void Standalone_mode() {
  if (Switch1_val>maxSequence) 
    Switch1_val=0;
  LED_sequence(Switch1_val);
}

// In demo mode, defined LED sequences auto-cycle every 8 seconds
void Demo_mode() {
  timer=millis();
  while (millis()<(timer+8000L))
    LED_sequence(Switch1_val);
  Switch1_val++; 
  if (Switch1_val>maxSequence) 
    Switch1_val=0; 
}

// In I2C mode, LED sequences are determined by parameters passed from Multiwii
void I2C_mode() {
  switch (param[0]) {
    case 'g': // MultiWii alarm mode
      LED_sequence(26);    
    break;
    case 'x': // Motors armed - ACRO mode
      LED_sequence(16);    
    break;
    case 'y': // Motors armed - HORIZON mode
      LED_sequence(12);    
    break;
    case 'w': // Motors armed - GPS RTH mode
      LED_sequence(25);
    break;
    case 'v': // Motors armed - GPS Pos Hold mode
      LED_sequence(22);
    break;
    case 'u': // Motors armed - Angle mode
      LED_sequence(12);    
    break;
    case 't': // Motors disarmed - Uncalibrated acc / not level
      LED_sequence(11);    
    break;
    case 's': // Motors disarmed - Parse other parameters
      // param[1]: 0=acro mode, 1=angle mode, 2=horizon mode
      // param[2]: 1=baro mode
      // param[3]: 1=mag mode
      // param[4]: 1=GPS hold, 2-rth
      // param[5]: value=(180-att.heading)/2  1 unit is 2 degrees.
      // param[6]: value=number of sats locked
      if (param[1]==0&&param[2]==0&&param[4]==0) //<--ACRO mode, safe to arm
        LED_sequence(15); 
      else {
        if (param[4]>0) { //<--GPS mode, disarmed, enable sat lock display
          param[6]=constrain(param[6],0,12);
          if (param[6]==0) //<--No sats locked
            LED_sequence(29); 
          else { //<--indicate number of locked sats
            set_all_rgb(0,0,0);
            if (param[6]<5)
              rgb=0; //<--less than 5 sats locked (red)
            else  
              rgb=1; //<--more than 5 sats locked (green) 
            for (int val=0;val<101;val++) { //<--breather function variant with multicolored LEDs
              var=(val*255/100);
              for (i=0;i<param[6];i++)
                set_led_unicolor(i,rgb,var);
              for (i=param[6];i<12;i++)
                set_led_rgb(i,var,var,var);
              wait(5);
            }
            for (int val=100;val>=0;val--) {
              var= uint8_t(val*255/100);
              for (i=0;i<param[6];i++)
                set_led_unicolor(i,rgb,var);
              for (i=param[6];i<12;i++)
                set_led_rgb(i,var,var,var);
              wait(5);
            }
          }
        }
        else 
          if (param[2]==1) { //<--BARO mode, cannot arm
            set_odd_rgb(255,0,0); 
            set_even_rgb(0,0,255);  
          } else {
            if (param[1]>0) { //<--ANGLE or HORIZON mode, safe to arm
              set_odd_rgb(0,255,0); 
              set_even_rgb(0,0,255);     
            }
          }
      }
      if (param[3]==1) { //<--MAG on, disarmed, enable alternating mag display 
#if defined(reverse_mag)
        magLED=90+param[5];
#else
        magLED=90-param[5];
#endif
        if (magLED<0) 
          magLED=magLED+180;
        memcpy(copybright,brightness,36);
        set_all_rgb(0,0,0);
        pointer(2,uint8_t(magLED*2*12/360));
        wait(1000);
        memcpy(brightness,copybright,36);
        wait(2000);
      }  
    break;
    case 'r': //<--MultiWii low batt warning
      for (led=0;led<12;led++)
        dblFlasher(led,255,0,0,200,400);
    break;
    default: //<--No interpretable I2C signal
      LED_sequence(0);
  }
  param[0]=0;
}

// read I2C message
void receiveEvent(int16_t n) {
  uint8_t p=0;  
  while(Wire.available()) {
    param[p++]=Wire.read();
    if (p>6) 
      p=6;
  }
}

// Replacement for delay() that doesn't screw up timers
void wait(unsigned long ms) { 
  unsigned long tt=millis();
  while ((millis()-tt)<ms);
}




























